﻿
using NetTopologySuite.Geometries;
using NetTopologySuite.Mathematics;
using NetTopologySuite.Operation.Distance3D;
using SuperMap.Data;
using System;
using System.Collections.Generic;
using GeometryNTS = NetTopologySuite.Geometries.Geometry;

namespace Ycjw.PipeLine.Map.Tools.Conditions.JJAnalysts
{
    public class DistBetweenLines
    {
        public static Coordinate[] NearestPoints(GeoLine3D geom1, GeoLine3D geom2)
        {
            return Distance3DOp.NearestPoints(Convert(geom1), Convert(geom2));
        }
        private static GeometryNTS Convert(GeoLine3D geoLine3D)
        {
            var lineStrings = new List<LineString>();
            for (var i = 0; i < geoLine3D.PartCount; i++)
            {
                var ls = new List<Coordinate>();
                var partPointsInLine1 = geoLine3D[i];
                for (var j = 0; j < partPointsInLine1.Count; j++)
                {
                    var seg1Point = new CoordinateZ() { X = partPointsInLine1[j].X, Y = partPointsInLine1[j].Y, Z = partPointsInLine1[j].Z };
                    ls.Add(seg1Point);
                }
                lineStrings.Add(new LineString(ls.ToArray()));
            }

            var result = new MultiLineString(lineStrings.ToArray(), GeometryFactory.Default);
            return result;
        }
        public class Point3D
        {
            public double X { get; set; }
            public double Y { get; set; }
            public double Z { get; set; }
            public Vector3D GetVector()
            {
                return new Vector3D(this.X, this.Y, this.Z);
            }
        }
        public class Line
        {
            public string Name { get; set; }
            public Point3D Start { get; set; } = new Point3D();
            public Point3D End { get; set; } = new Point3D();
            public double Length
            {
                get
                {
                    return Math.Sqrt(Math.Pow((End.X - Start.X), 2) + Math.Pow((End.Y - Start.Y), 2));
                }
            }
        }
        public static Point3D ClampPointToLine(Point3D pointToClamp, Line lineToClampTo)
        {
            Point3D clampedPoint = new Point3D();
            double minX, minY, minZ, maxX, maxY, maxZ;
            if (lineToClampTo.Start.X <= lineToClampTo.End.X)
            {
                minX = lineToClampTo.Start.X;
                maxX = lineToClampTo.End.X;
            }
            else
            {
                minX = lineToClampTo.End.X;
                maxX = lineToClampTo.Start.X;
            }
            if (lineToClampTo.Start.Y <= lineToClampTo.End.Y)
            {
                minY = lineToClampTo.Start.Y;
                maxY = lineToClampTo.End.Y;
            }
            else
            {
                minY = lineToClampTo.End.Y;
                maxY = lineToClampTo.Start.Y;
            }
            if (lineToClampTo.Start.Z <= lineToClampTo.End.Z)
            {
                minZ = lineToClampTo.Start.Z;
                maxZ = lineToClampTo.End.Z;
            }
            else
            {
                minZ = lineToClampTo.End.Z;
                maxZ = lineToClampTo.Start.Z;
            }
            clampedPoint.X = (pointToClamp.X < minX) ? minX : (pointToClamp.X > maxX) ? maxX : pointToClamp.X;
            clampedPoint.Y = (pointToClamp.Y < minY) ? minY : (pointToClamp.Y > maxY) ? maxY : pointToClamp.Y;
            clampedPoint.Z = (pointToClamp.Z < minZ) ? minZ : (pointToClamp.Z > maxZ) ? maxZ : pointToClamp.Z;
            return clampedPoint;
        }
        public static Line CalculateDistBetweenLines(Line l1, Line l2)
        {
            Vector3D p1, p2, p3, p4, d1, d2;
            p1 = l1.Start.GetVector();
            p2 = l1.End.GetVector();
            p3 = l2.Start.GetVector();
            p4 = l2.End.GetVector();
            d1 = p2 - p1;
            d2 = p4 - p3;
            double eq1nCoeff = (d1.X * d2.X) + (d1.Y * d2.Y) + (d1.Z * d2.Z);
            double eq1mCoeff = (-(Math.Pow(d1.X, 2)) - (Math.Pow(d1.Y, 2)) - (Math.Pow(d1.Z, 2)));
            double eq1Const = ((d1.X * p3.X) - (d1.X * p1.X) + (d1.Y * p3.Y) - (d1.Y * p1.Y) + (d1.Z * p3.Z) - (d1.Z * p1.Z));
            double eq2nCoeff = ((Math.Pow(d2.X, 2)) + (Math.Pow(d2.Y, 2)) + (Math.Pow(d2.Z, 2)));
            double eq2mCoeff = -(d1.X * d2.X) - (d1.Y * d2.Y) - (d1.Z * d2.Z);
            double eq2Const = ((d2.X * p3.X) - (d2.X * p1.X) + (d2.Y * p3.Y) - (d2.Y * p2.Y) + (d2.Z * p3.Z) - (d2.Z * p1.Z));
            double[,] M = new double[,] { { eq1nCoeff, eq1mCoeff, -eq1Const }, { eq2nCoeff, eq2mCoeff, -eq2Const } };
            int rowCount = M.GetUpperBound(0) + 1;
            // pivoting
            for (int col = 0; col + 1 < rowCount; col++)
            {
                if (M[col, col] == 0)
                // check for zero coefficients
                {
                    // find non-zero coefficient
                    int swapRow = col + 1;
                    for (; swapRow < rowCount; swapRow++) if (M[swapRow, col] != 0) break;

                    if (M[swapRow, col] != 0) // found a non-zero coefficient?
                    {
                        // yes, then swap it with the above
                        double[] tmp = new double[rowCount + 1];
                        for (int i = 0; i < rowCount + 1; i++)
                        { 
                            tmp[i] = M[swapRow, i]; 
                            M[swapRow, i] = M[col, i];
                            M[col, i] = tmp[i]; 
                        }
                    }
                    else return null; // no, then the matrix has no unique solution
                }
            }
            // elimination
            for (int sourceRow = 0; sourceRow + 1 < rowCount; sourceRow++)
            {
                for (int destRow = sourceRow + 1; destRow < rowCount; destRow++)
                {
                    double df = M[sourceRow, sourceRow];
                    double sf = M[destRow, sourceRow];
                    for (int i = 0; i < rowCount + 1; i++)
                        M[destRow, i] = M[destRow, i] * df - M[sourceRow, i] * sf;
                }
            }

            // back-insertion
            for (int row = rowCount - 1; row >= 0; row--)
            {
                double f = M[row, row];
                if (f == 0) return null;

                for (int i = 0; i < rowCount + 1; i++) 
                    M[row, i] /= f;
                for (int destRow = 0; destRow < row; destRow++)
                { 
                    M[destRow, rowCount] -= M[destRow, row] * M[row, rowCount]; 
                    M[destRow, row] = 0; 
                }
            }
            double n = M[0, 2];
            double m = M[1, 2];
            Point3D i1 = new Point3D { X = p1.X + (m * d1.X), Y = p1.Y + (m * d1.Y), Z = p1.Z + (m * d1.Z) };
            Point3D i2 = new Point3D { X = p3.X + (n * d2.X), Y = p3.Y + (n * d2.Y), Z = p3.Z + (n * d2.Z) };
            Point3D i1Clamped = ClampPointToLine(i1, l1);
            Point3D i2Clamped = ClampPointToLine(i2, l2);
            return new Line { Start = i1Clamped, End = i2Clamped };
        }

        public override string ToString()
        {
            return base.ToString();
        }

        public override bool Equals(object obj)
        {
            return base.Equals(obj);
        }

        public override int GetHashCode()
        {
            return base.GetHashCode();
        }
    }
}
